#include <webots/robot.h>
#include <webots/distance_sensor.h>
#include <webots/motor.h>
#include "../auv_ctrl/auv_ctrl.hpp"
// time in [ms] of a simulation step
#define TIME_STEP 64

#define MAX_SPEED 12.56
HANDLE comHandle = INVALID_HANDLE_VALUE;

typedef enum
{
  LEFT_MOTOR = 0,
  RIGHT_MOTOR,
  MOTOR_NUM
}motor_enum_t;

//打开串口
HANDLE OpenSerial(const char *com, //串口名称，如COM1，COM2
    int baud,       //波特率：常用取值：CBR_9600、CBR_19200、CBR_38400、CBR_115200、CBR_230400、CBR_460800
    int byteSize,   //数位大小：可取值7、8；
    int parity,     //校验方式：可取值NOPARITY、ODDPARITY、EVENPARITY、MARKPARITY、SPACEPARITY
    int stopBits)   //停止位：ONESTOPBIT、ONE5STOPBITS、TWOSTOPBITS；
{    
    DCB dcb;
    BOOL b = FALSE;
	COMMTIMEOUTS CommTimeouts;	
    HANDLE comHandle = INVALID_HANDLE_VALUE;

    //打开串口
	comHandle = CreateFile(com,            //串口名称
		GENERIC_READ | GENERIC_WRITE,      //可读、可写   				 
		0,            // No Sharing                               
		NULL,         // No Security                              
		OPEN_EXISTING,// Open existing port only                     
		FILE_ATTRIBUTE_NORMAL,            // Non Overlapped I/O                           
		NULL);        // Null for Comm Devices

	if (INVALID_HANDLE_VALUE == comHandle) {
        LOGE("CreateFile fail\r\n");
        return comHandle;
    }

    // 设置读写缓存大小
	b = SetupComm(comHandle, BUF_SIZE, BUF_SIZE);
    if (!b) {
        LOGE("SetupComm fail\r\n");
    }

    //设定读写超时
	CommTimeouts.ReadIntervalTimeout = MAXDWORD;//读间隔超时
	CommTimeouts.ReadTotalTimeoutMultiplier = 0;//读时间系数
	CommTimeouts.ReadTotalTimeoutConstant = 0;//读时间常量
	CommTimeouts.WriteTotalTimeoutMultiplier = 1;//写时间系数
	CommTimeouts.WriteTotalTimeoutConstant = 1;//写时间常量
	b = SetCommTimeouts(comHandle, &CommTimeouts); //设置超时
    if (!b) {
        LOGE("SetCommTimeouts fail\r\n");
    }

    //设置串口状态属性
    GetCommState(comHandle, &dcb);//获取当前
	dcb.BaudRate = baud; //波特率
	dcb.ByteSize = byteSize; //每个字节有位数
	dcb.Parity   = parity; //无奇偶校验位
	dcb.StopBits = stopBits; //一个停止位
	b = SetCommState(comHandle, &dcb);//设置
    if (!b) {
        LOGE("SetCommState fail\r\n");
    }

    return comHandle;
}

// entry point of the controller
int main(int argc, char **argv) {
  
  // const char *com = "COM3";
  // comHandle = OpenSerial(com, CBR_115200, 8, NOPARITY, ONESTOPBIT);
  // if (INVALID_HANDLE_VALUE == comHandle) {
    // LOGE("[hello_world]OpenSerial COM3 fail!\r\n");
    // return -1;
  // }
  // else{
    // LOGD("[hello_world]Open COM3 Successfully!\r\n");  
  // }

  // initialize the Webots API
  wb_robot_init();

  // internal variables
  int i;
  WbDeviceTag ps[8],motors[MOTOR_NUM];
  char ps_names[8][4] = {
    "ps0", "ps1", "ps2", "ps3",
    "ps4", "ps5", "ps6", "ps7"
  };

  // initialize devices
  for (i = 0; i < 8 ; i++) {
    ps[i] = wb_robot_get_device(ps_names[i]);
    wb_distance_sensor_enable(ps[i], TIME_STEP);
  }
  
  char* motor_name[2] = {"left wheel motor","right wheel motor"};
  // initialize motors
  for (i = 0; i < 2 ; i++) {
     motors[i] = wb_robot_get_device(motor_name[i]);
  }
 
  wb_motor_set_position(motors[LEFT_MOTOR], INFINITY);
  wb_motor_set_position(motors[RIGHT_MOTOR], INFINITY);
  wb_motor_set_velocity(motors[LEFT_MOTOR], 0.0);
  wb_motor_set_velocity(motors[RIGHT_MOTOR], 0.0);

  // feedback loop: step simulation until an exit event is received
  while (wb_robot_step(TIME_STEP) != -1) {
    // read sensors outputs
    double ps_values[8];
    for (i = 0; i < 8 ; i++)
      ps_values[i] = wb_distance_sensor_get_value(ps[i]);

    // detect obstacles
    bool right_obstacle =
      ps_values[0] > 100.0 ||
      ps_values[1] > 100.0 ||
      ps_values[2] > 100.0;
    bool left_obstacle =
      ps_values[5] > 100.0 ||
      ps_values[6] > 100.0 ||
      ps_values[7] > 100.0;

    // initialize motor speeds at 50% of MAX_SPEED.
    double left_speed  = 0.5 * MAX_SPEED;
    double right_speed = 0.5 * MAX_SPEED;

    // modify speeds according to obstacles
    if (left_obstacle) {
      // turn right
      left_speed  = 0.5 * MAX_SPEED;
      right_speed = -0.5 * MAX_SPEED;
    }
    else if (right_obstacle) {
      // turn left
      left_speed  = -0.5 * MAX_SPEED;
      right_speed = 0.5 * MAX_SPEED;
    }

    // write actuators inputs
    wb_motor_set_velocity(motors[LEFT_MOTOR], left_speed);
    wb_motor_set_velocity(motors[RIGHT_MOTOR], right_speed);
  }

  // cleanup the Webots API
  wb_robot_cleanup();
  return 0; //EXIT_SUCCESS
}